//
// Created by wenbo on 24-9-7.
//
#include "drv_mrc.h"
#include "Array_user.h"

float demagnetization_buffer[DEMAGNETIZATION_BUFFER_SIZE] = {
    -4.8, -4.6586, -4.441, -4.1542, -3.8061, -3.4053, -2.9611, -2.4831, -1.9812,
    -1.4651, -0.9445, -0.42879, 0.073167, 0.5531, 1.0035, 1.4176, 1.7896, 2.1147,
    2.3892, 2.6103, 2.7762, 2.8864, 2.9413, 2.942, 2.891, 2.7911, 2.6462,
    2.4607, 2.2395, 1.9881, 1.712, 1.4171, 1.1095, 0.79509, 0.47963, 0.16876,
    -0.13224, -0.41847, -0.68551, -0.92945, -1.1469, -1.3353, -1.4923, -1.6165, -1.7072,
    -1.764, -1.7875, -1.7785, -1.7385, -1.6696, -1.5741, -1.4549, -1.3149, -1.1576,
    -0.9864, -0.8049, -0.61673, -0.42546, -0.23461, -0.047504, 0.13271, 0.30313, 0.46118,
    0.60458, 0.73141, 0.84012, 0.92955, 0.99891, 1.0478, 1.0762, 1.0846, 1.0734,
    1.0438, 0.99711, 0.93474, 0.85849, 0.77028, 0.67214, 0.56622, 0.4547, 0.33979,
    0.22364, 0.10835, -0.0040912, -0.11181, -0.21311, -0.30648, -0.39059, -0.46434, -0.52688,
    -0.57755, -0.61596, -0.64193, -0.65553, -0.65702, -0.64688, -0.62577, -0.59451, -0.55407,
    -0.50555, -0.45013, -0.38907, -0.32368, -0.2553, -0.18524, -0.11481, -0.045275, 0.022187,
    0.086472, 0.14658, 0.20162, 0.25084, 0.29361, 0.32944, 0.35799, 0.37905, 0.39257,
    0.39862, 0.3974, 0.38923, 0.37455, 0.35387, 0.32782, 0.29707, 0.26236, 0.22447,
    0.18419, 0.14232, 0.099685, 0.057051, 0.015175, -0.025239, -0.063539, -0.099138, -0.13152,
    -0.16025, -0.18497, -0.20541, -0.22139, -0.23281, -0.23966, -0.24201, -0.24, -0.23384,
    -0.22382, -0.21028, -0.19359, -0.17418, -0.1525, -0.12904, -0.10427, -0.078688, -0.052779,
    -0.02701, -0.00183, 0.022342, 0.045122, 0.066166, 0.085176, 0.1019, 0.11614, 0.12775,
    0.13662, 0.14273, 0.14607, 0.14669, 0.14471, 0.14027, 0.13353, 0.12473, 0.11409,
    0.10188, 0.08838, 0.073883, 0.058683, 0.043078, 0.02736, 0.011808, -0.0033087, -0.017743,
    -0.031268, -0.043684, -0.054818, -0.064527, -0.072698, -0.079252, -0.084139, -0.087342, -0.088876,
    -0.08878, -0.087126, -0.084005, -0.079534, -0.073848, -0.067095, -0.05944, -0.051055, -0.042117,
    -0.032806, -0.023303, -0.013782, -0.0044108, 0.0046505, 0.013256, 0.021272, 0.028582, 0.035087,
    0.040706, 0.045375, 0.049053, 0.051715, 0.053356, 0.05399, 0.053647, 0.052373, 0.05023,
    0.047291, 0.04364, 0.039372, 0.034587, 0.02939, 0.023891, 0.018198, 0.012419, 0.0066609,
    0.0010231, -0.0043997, -0.0095211, -0.014263, -0.018558, -0.022349, -0.02559, -0.028246, -0.030295,
    -0.031726, -0.032539, -0.032746, -0.032367, -0.031434, -0.029986, -0.02807, -0.025739, -0.02305,
    -0.020067, -0.016854, -0.013477, -0.010001, -0.0064937, -0.0030166, 0.00037004, 0.0036102, 0.006653,
    0.0094529, 0.011971, 0.014174};
float magnetization_buffer[MAGNETIZATION_BUFFER_SIZE] = {
    0, 0.088379, 0.22438, 0.40364, 0.62122, 0.8717, 1.1493, 1.448, 1.7618, 2.0843, 2.4097, 2.732, 3.0457,
    3.3457, 3.6272, 3.886, 4.1185, 4.3217, 4.4932, 4.6314, 4.7351, 4.804, 4.8383, 4.8388, 4.8069, 4.7444,
    4.6539, 4.5379, 4.3997, 4.2425, 4.07, 3.8857, 3.6935, 3.4969, 3.2998, 3.1055, 2.9174, 2.7385, 2.5716,
    2.4191, 2.2832, 2.1655, 2.0673, 1.9897, 1.933, 1.8975, 1.8828, 1.8885, 1.9134, 1.9565, 2.0162, 2.0907,
    2.1782, 2.2765, 2.3835, 2.4969, 2.6145, 2.7341, 2.8534, 2.9703, 3.0829, 3.1895, 3.2882, 3.3779, 3.4571,
    3.5251, 3.581, 3.6243, 3.6549, 3.6727, 3.6778, 3.6709, 3.6524, 3.6232, 3.5842, 3.5366, 3.4814, 3.4201,
    3.3539, 3.2842, 3.2124, 3.1398, 3.0677, 2.9974, 2.9301, 2.8668, 2.8085, 2.7559, 2.7098, 2.6707, 2.639,
    2.615, 2.5988, 2.5903, 2.5894, 2.5957, 2.6089, 2.6284, 2.6537, 2.684, 2.7187, 2.7568, 2.7977, 2.8404,
    2.8842, 2.9282, 2.9717, 3.0139, 3.054, 3.0916, 3.126, 3.1568, 3.1835, 3.2059, 3.2237, 3.2369, 3.2454,
    3.2491, 3.2484, 3.2433, 3.2341, 3.2212, 3.2049, 3.1857, 3.164, 3.1403, 3.1151, 3.089, 3.0623, 3.0357,
    3.0095, 2.9842, 2.9603, 2.938, 2.9178, 2.8998, 2.8844, 2.8716, 2.8616, 2.8545, 2.8502, 2.8487, 2.85,
    2.8538, 2.8601, 2.8686, 2.879, 2.8911, 2.9047, 2.9194, 2.9348, 2.9508, 2.967, 2.9831, 2.9989, 3.014,
    3.0282, 3.0414, 3.0532, 3.0637, 3.0726, 3.0798, 3.0854, 3.0892, 3.0913, 3.0917, 3.0904, 3.0877, 3.0835,
    3.078, 3.0713, 3.0637, 3.0552, 3.0462, 3.0367, 3.0269, 3.0171, 3.0074, 2.9979, 2.9889, 2.9805, 2.9727,
    2.9657, 2.9597, 2.9546, 2.9505, 2.9474, 2.9454, 2.9445, 2.9445, 2.9455, 2.9475, 2.9503, 2.9538, 2.9581,
    2.9628, 2.9681, 2.9737, 2.9795, 2.9854, 2.9914, 2.9972, 3.0029, 3.0083, 3.0133, 3.0179, 3.0219, 3.0254,
    3.0284, 3.0307, 3.0323, 3.0333, 3.0337, 3.0335, 3.0327, 3.0314, 3.0296, 3.0273, 3.0246, 3.0216, 3.0184,
    3.0149, 3.0114, 3.0078, 3.0042, 3.0006, 2.9973, 2.994, 2.9911, 2.9884, 2.986, 2.984, 2.9823, 2.9811,
    2.9802, 2.9797, 2.9795, 2.9798, 2.9804, 2.9813, 2.9825, 2.9839, 2.9856, 2.9875, 2.9895, 2.9916, 2.9937,
    2.9959, 2.9981, 3.0002, 3.0023, 3.0042, 3.0059, 3.0075, 3.0089

};

uint16_t prescaler = 9 - 1;       // Set the prescaler
uint64_t tim_clk_freq = 72000000; // Set the timer clock frequency
volatile const float mrc_supply_voltage_scale_index = 2.5 / 3.0;

#define MRC_PWM_FREQ 20000
#define MRC_MAX_VOLTAGE 5.0f
#define ADC_OVER_SAMPLING_RATIO 16

/**
 * @brief Initialize the MRC device.
 *
 * This function initializes the MRC device by setting its device name, input channels, timer handle, and logging
 * function. It also starts the PWM for the given timer and channels and sets the PWM parameters to 0.
 *
 * @param dev_name A pointer to the device name.
 * @param MRC A pointer to the MRC device structure.
 * @param htim A pointer to the timer handle.
 * @param IN1 The first input channel.
 * @param IN2 The second input channel.
 */
#ifdef AT8236
__WEAK void MRC_Init(const uint8_t *dev_name, Device_MRC_t *MRC, TIM_HandleTypeDef *htim, uint32_t IN1, uint32_t IN2)
{
    MRC->device_name = dev_name;
    MRC->IN1 = IN1;
    MRC->IN2 = IN2;
    MRC->pwm_handle = htim;
    MRC->LOG_MSG = printf;
    HAL_TIM_PWM_Start(htim, IN1);
    HAL_TIM_PWM_Start(htim, IN2);
    MRC_set_pwm_param(htim, IN1, MRC_PWM_FREQ, 0);
    MRC_set_pwm_param(htim, IN1, MRC_PWM_FREQ, 0);
    printf("device MRC(%s) init success!\n", dev_name);
}
#endif

#ifdef VNH7040

/**
 * @brief Initialize the MRC device
 *
 * This function initializes the MRC device, including setting the device name, timer handle, PWM channel, INA and INB
 * pins, etc.
 *
 * @param dev_name Device name
 * @param MRC Pointer to the MRC device structure
 * @param htim Timer handle
 * @param INA GPIO port for INA pin
 * @param INA_PIN GPIO pin number for INA pin
 * @param INB GPIO port for INB pin
 * @param INB_PIN GPIO pin number for INB pin
 * @param PWM PWM channel
 *
 * @details
 * The function first assigns the passed parameters to the MRC device structure, then starts the PWM channel of the
 * timer, sets the PWM parameters, and enables the INA and INB pins. Next, the function starts ADC calibration and DMA
 * transfer, and finally prints a message indicating successful device initialization.
 *
 * @note
 * This function assumes that the ADC_BUFFER array has been defined and contains two ADC values.
 *
 * @code
 * void MRC_Init(const uint8_t *dev_name, Device_MRC_t *MRC, TIM_HandleTypeDef *htim,
 *              GPIO_TypeDef * INA, uint16_t INA_PIN,
 *              GPIO_TypeDef * INB,  uint16_t INB_PIN,
 *              uint32_t PWM)
 * {
 *     MRC->device_name = dev_name;
 *     MRC->pwm_handle = htim;
 *     MRC->PWM = PWM;                                         //PWM channel
 *     MRC->INA = INA;                                         //INA GPIO
 *     MRC->INA_PIN = INA_PIN;                                 //INA PIN
 *
 *     MRC->INB = INB;                                         //INB GPIO
 *     MRC->INB_PIN = INB_PIN;                                 //INB PIN
 *
 *     MRC->LOG_MSG = printf;
 *     HAL_TIM_PWM_Start(htim, PWM);
 *     MRC_set_pwm_param(htim, PWM, MRC_PWM_FREQ, 0);
 *     HAL_GPIO_WritePin(INA,INA_PIN,GPIO_PIN_SET);
 *     HAL_GPIO_WritePin(INB,INB_PIN,GPIO_PIN_RESET);
 *
 *     HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET_LINEARITY,ADC_SINGLE_ENDED);
 *     HAL_ADC_Start_DMA(&hadc1,(uint32_t *)&ADC_BUFFER,2);
 *
 *     printf("device MRC(%s) init success!\n", dev_name);
 * }
 * @endcode
 */
void MRC_Init(uint8_t const *dev_name, Device_MRC_t *MRC, TIM_HandleTypeDef *htim, GPIO_TypeDef *INA, uint16_t INA_PIN,
              GPIO_TypeDef *INB, uint16_t INB_PIN, uint32_t PWM)
{

    MRC->device_name = dev_name;
    MRC->pwm_handle = htim; // PWM timer handle
    MRC->PWM = PWM;         // PWM channel
    MRC->INA = INA;         // INA GPIO
    MRC->INA_PIN = INA_PIN; // INA PIN

    MRC->INB = INB;         // INB GPIO
    MRC->INB_PIN = INB_PIN; // INB PIN

    MRC->state = Disengagement;
#ifdef LOG_MODE
    MRC->LOG_MSG = printf;
#endif
    HAL_TIM_PWM_Start(htim, PWM);
    MRC_set_pwm_param(htim, PWM, MRC_PWM_FREQ, 0);
    /* OUTA enable OUTB disable*/
    HAL_GPIO_WritePin(INA, INA_PIN, GPIO_PIN_SET);
    HAL_GPIO_WritePin(INB, INB_PIN, GPIO_PIN_RESET);

    // MRC->xCoilCrtlSemaphore = xSemaphoreCreateBinary(); //create semaphore

#ifdef STM32H7
    HAL_ADCEx_Calibration_Start(&hadc1, ADC_CALIB_OFFSET_LINEARITY, ADC_SINGLE_ENDED);

#endif

#ifdef STM32F1
    HAL_ADCEx_Calibration_Start(&hadc1);
#endif

    scaleArray(demagnetization_buffer, DEMAGNETIZATION_BUFFER_SIZE, mrc_supply_voltage_scale_index);
    scaleArray(magnetization_buffer, MAGNETIZATION_BUFFER_SIZE, mrc_supply_voltage_scale_index);
#ifdef LOG_MODE
    printf("device MRC(%s) init success!\n", dev_name);
#endif
}

/**
 * @brief Process the ADC values for VNH7070_Multisense_ADC
 *
 * This function converts the values in the ADC_BUFFER array to actual des_voltage values and stores them in the MRC1 and
 * MRC2 structures.
 *
 * @details
 * The function calculates the actual des_voltage value by multiplying the value in ADC_BUFFER by 3.3 and dividing by 4095,
 * and then stores the result in the actual_voltage member of MRC1 and MRC2.
 *
 * @note
 * The ADC_BUFFER array should contain two ADC values, corresponding to the des_voltage of MRC1 and MRC2.
 *
 * @code
 * void VNH7070_Multisense_ADC_process(void)
 * {
 *     MRC1.actual_voltage = ADC_BUFFER[0] * 3.3 / 4095;
 *     MRC2.actual_voltage = ADC_BUFFER[1] * 3.3 / 4095;
 * }
 * @endcode
 */
void VNH7070_Multisense_ADC_process(Device_MRC_t *MRC1, Device_MRC_t *MRC2, float *ADC_BUFFER)
{
    MRC1->actual_voltage = (float)ADC_BUFFER[0] * 3.3f / (4095 * ADC_OVER_SAMPLING_RATIO);
    MRC2->actual_voltage = (float)ADC_BUFFER[1] * 3.3f / (4095 * ADC_OVER_SAMPLING_RATIO);
}

#endif

/**
 * @brief Set PWM parameters.
 *
 * This function sets the PWM parameters for a given timer and channel. It calculates the prescaler, timer clock
 * frequency, PWM frequency, and PWM duty cycle pulse based on the input parameters. It then sets the timer prescaler,
 * auto-reload value, and compare value accordingly.
 *
 * @param htim A pointer to the timer handle.
 * @param Channel The channel for which the PWM parameters are to be set.
 * @param freq The desired PWM frequency.
 * @param duty The desired PWM duty cycle.
 */
void MRC_set_pwm_param(TIM_HandleTypeDef *htim, uint32_t Channel, uint32_t freq, uint16_t duty)
{
    float pwm_freq_arr = (tim_clk_freq * 1.0) / (prescaler + 1) / freq * 1.0 - 1; // Calculate PWM frequency
    float pwm_duty_pulse = duty * 1.0 / 100 * (pwm_freq_arr + 1);                 // Calculate PWM duty cycle pulse

    __HAL_TIM_SET_PRESCALER(htim, prescaler);                      // Set the timer prescaler
    __HAL_TIM_SetAutoreload(htim, (uint16_t)pwm_freq_arr);         // Set the timer auto-reload value
    __HAL_TIM_SetCompare(htim, Channel, (uint16_t)pwm_duty_pulse); // Set the timer compare value

    // Print PWM frequency and duty cycle pulse
    // printf("pwm_freq_arr:%.2f\r\n", pwm_freq_arr);
    // printf("pwm_duty_pulse:%.2f\r\n", pwm_duty_pulse);
}

/**
 * @brief Set the des_voltage of the MRC device.
 *
 * This function sets the des_voltage of the MRC device. It calculates the duty cycle based on the input des_voltage and sets
 * the PWM parameters accordingly. If the des_voltage is greater than 5.0V, it logs a message indicating that the des_voltage is
 * too high.
 *
 * @param des_voltage The des_voltage to be set for the MRC device.
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_set_voltage(float des_voltage, Device_MRC_t *MRC)
{
    uint16_t duty;

    if (des_voltage <= MRC_MAX_VOLTAGE)
    {
        MRC->actual_voltage = des_voltage;

        if (des_voltage >= 0.0f)
        {
            duty = (uint16_t)((des_voltage / MRC_MAX_VOLTAGE) * 100);
#ifdef AT8236
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN1, MRC_PWM_FREQ, duty);
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN2, MRC_PWM_FREQ, 100);
            // MRC->LOG_MSG("MRC_set_voltage: %.2f\r\n", des_voltage);
#endif

#ifdef VNH7040
            MRC_set_pwm_param(MRC->pwm_handle, MRC->PWM, MRC_PWM_FREQ, duty);
            /* OUTA enable OUTB disable*/
            HAL_GPIO_WritePin(MRC->INA, MRC->INA_PIN, GPIO_PIN_SET);
            HAL_GPIO_WritePin(MRC->INB, MRC->INB_PIN, GPIO_PIN_RESET);

#endif
        }
        else if (des_voltage < 0.0f)
        {
            duty = (uint16_t)(-(des_voltage / MRC_MAX_VOLTAGE) * 100);

#ifdef AT8236
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN1, MRC_PWM_FREQ, 100);
            MRC_set_pwm_param(MRC->pwm_handle, MRC->IN2, MRC_PWM_FREQ, duty);
// MRC->LOG_MSG("MRC_set_voltage: %.2f\r\n", des_voltage);
#endif

#ifdef VNH7040

            MRC_set_pwm_param(MRC->pwm_handle, MRC->PWM, MRC_PWM_FREQ, duty);
            /* OUTA disable OUTB enable*/
            HAL_GPIO_WritePin(MRC->INA, MRC->INA_PIN, GPIO_PIN_RESET);
            HAL_GPIO_WritePin(MRC->INB, MRC->INB_PIN, GPIO_PIN_SET);

#endif
        }
    }
    else
    {
        MRC->LOG_MSG("MRC_set_voltage: The setted des_voltage is too high: %.2f\r\n", des_voltage);
    }
}

/**
 * @brief Lock the MRC device.
 *
 * This function locks the MRC device by setting its state to Magnetization and starting the timer interrupt.
 *
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_lock(Device_MRC_t *MRC)
{
    MRC->state = Magnetization;
    HAL_TIM_Base_Start_IT(&htim4);
}

/**
 * @brief Unlock the MRC device.
 *
 * This function unlocks the MRC device by setting its state to Demagnetization and starting the timer interrupt.
 *
 * @param MRC A pointer to the MRC device structure.
 */
void MRC_unlock(Device_MRC_t *MRC)
{
    MRC->state = Demagnetization;
    HAL_TIM_Base_Start_IT(&htim4);
}

void MRC_switch(Device_MRC_t *MRC, TIM_HandleTypeDef *htim)
{
    if (MRC->state == Demagnetization)
    {
        MRC_set_voltage(demagnetization_buffer[MRC->demagnetization_counter], MRC);
        // printf("Magnetization des_voltage: %f\n",demagnetization_buffer[MRC->magnetization_counter]);
        MRC->demagnetization_counter++;
        if (MRC->demagnetization_counter == DEMAGNETIZATION_BUFFER_SIZE)
        {
            MRC->demagnetization_counter = 0;
            MRC->state = Disengagement;
            HAL_TIM_Base_Stop_IT(htim);
        }
    }
    else if (MRC->state == Magnetization)
    {
        MRC_set_voltage(magnetization_buffer[MRC->magnetization_counter], MRC);
        // printf("Magnetization des_voltage: %f\n",demagnetization_buffer[MRC->magnetization_counter]);
        MRC->magnetization_counter++;
        if (MRC->magnetization_counter == MAGNETIZATION_BUFFER_SIZE)
        {
            MRC->magnetization_counter = 0;
            MRC->state = Engagement;
            HAL_TIM_Base_Stop_IT(htim);
        }
    }
}
